// variometer.c
//

#include <avr/io.h>
#include <stdio.h>

#include "../mcu/config.h"
#include "../mcu/types.h"
#include "../mcu/twi_master.h"
#include "../mcu/serial.h"
#include "../mcu/delay.h"
#include "../mcu/timer.h"

#include "common.h"
#include "bmp085.h"
#include "variometer.h"
#include "beep.h"
#include "pztbl15.h"


int gnAcqTicks;
int gnSmpCnt;
s32 gZBuf[SNS_NUM_Z_SAMPLES];
s32 gnAltAvgM; 
s32 gnAltAvgCm;
s32 gnAltCm;
s32 gnCps;
u32 gnTraw;
u32 gnPraw;
s16 gnTempC;
s64 gnSum_t;
s64 gnSum_t2;
s64 gnDen;

u32 p_lpf;

#if UART_DBG
s8 gszBuf[128];
#endif

#if SENSOR_POST_LPF
int	gnKLPFC = SENSOR_DFL_KLPFC;
s32 gnCpsx32;
#endif

volatile u8 gnTimerHrs;
volatile u8 gnTimerMin;
volatile u8 gnTimerSec;

volatile u32 gnTimeTick;
volatile u16 gnSecTick;

volatile u8  gnRealHrs;
volatile u8  gnRealMin;
volatile u8  gnRealSec;

s16 gnLaunchAltM;
u8 gnPZTblSel = 12;
u8 gbTimerEn;


void prs_Init(void)
{
	gnSmpCnt = 0;
	gnAltAvgM = 0;
	gnAltCm = 0;
	gnCps = 0;
	gnTempC = 0;
	// following parameters used in linear regression computation
	// for climb and sink rate, need to be computed only once.
	gnSum_t = -((s64)SNS_NUM_Z_SAMPLES * (SNS_NUM_Z_SAMPLES-1))/2;
	gnSum_t2 = ((s64)SNS_NUM_Z_SAMPLES * (SNS_NUM_Z_SAMPLES-1)*(2*SNS_NUM_Z_SAMPLES-1))/6;
	gnDen = ((s64)SNS_NUM_Z_SAMPLES*gnSum_t2) - (gnSum_t*gnSum_t);

#ifdef SENSOR_UNIT_TEST
	prs_UnitTest();
#endif
}


void prs_InitData()  
{
	prs_InitWindowBuffers();
#if SENSOR_POST_LPF
	gnCpsx32 = 0;
#endif
	gnCps = 0;
	gnSmpCnt = SNS_NUM_Z_SAMPLES-1;

//	prs_TriggerTemperatureSample();
	gnAcqTicks = 0;
}


#ifdef SENSOR_UNIT_TEST

// Bosch SensorTec test to validate the code used to compute temperature
// and pressure from pressure sensor readings 

u16 ac5[9] = {32758, 32758, 32758, 30774, 30774, 30774, 26431, 26431, 26431};
u16 ac6[9] = {23153, 23153, 23153, 9469, 9469, 9469, 40099, 40099, 40099};

u32 up[9] = {25827, 23843, 21476, 25634, 23656, 21302, 26060, 24056, 21657};
u32 ut[9] = {24524, 27898, 34787, 9389, 14520, 21853, 40438, 45980, 54518};

s32  pResult[9]= {70369, 69964, 69961, 66140, 69406, 69385, 68346, 70599, 70559 };
s16  tResult[9] = {-177, 150, 650, -404, 150, 650, -338, 150, 650};

void prs_UnitTest(void)
{
	int cnt;
	s16 t;
	s32 p;

	gAC1 = 408;
	gAC2 = -72;
	gAC3 = -14383;
	gAC4 = 32741;
	gB1 = 6190;
	gB2 = 4;
	gMb = 0;
	gMc = -8711;
	gMd = 2868;
	for (cnt = 0; cnt < 9; cnt++)
	{
		gAC5 = ac5[cnt];
		gAC6 = ac6[cnt];
		gnTraw = ut[cnt];
		gnPraw = up[cnt];
		t = prs_CalcTemperatureCx10();
		p = prs_CalcPressurePa();
		sprintf(gszBuf,"%d T %d %d P %ld %ld\r\n",cnt, tResult[cnt],t, pResult[cnt], p);
		uart_puts(gszBuf);
	}
}
#endif // SENSOR_UNIT_TEST


s16 gnDispTick;

#define LPF_K	4

void prs_SampleContinuous(void)
{
	s16 t;
	s32 p;

//	if( ! (BMP085_PIN & (1 << BMP085_EOS)) )
//		return;

	t = bmp085_calc_temperature(bmp085_measure_temperature());
	p = bmp085_calc_pressure(bmp085_measure_pressure());

	p_lpf = p_lpf * (32 - LPF_K) + (p << 5) * LPF_K;
	p_lpf = ((p_lpf + 16) >> 5);

	p = p_lpf >> 5;

	gnTempC = (t >= 0?  (t + 5) / 10 : (t - 5) / 10);
	gZBuf[gnSmpCnt] = gnAltCm = prs_Pa2Cm(p);

	gnSmpCnt = (gnSmpCnt + 1) % SNS_NUM_Z_SAMPLES;

	prs_AverageAltitude();
	prs_DeltaCmPerSec();
#if SENSOR_POST_LPF
	prs_LPFClimbRate();
#endif // SENSOR_POST_LPF

	printf("gnCps : %ld\n", gnCps);
	beep_SetCps(gnCps);
	wait_1ms(1);
}


void prs_InitWindowBuffers()   
{
	int n;

	prs_AcquireAveragedSample(10);

	n = SNS_NUM_Z_SAMPLES;
	while(n--) 
		gZBuf[n] = gnAltCm;
}


void prs_ProfileComputationTime(void)
{
	/*
	s32 p;
	s16 t;
//	tmr1_Start();
	t = prs_CalcTemperatureCx10();
	gnTempC = (t >= 0?  (t+5)/10 : (t-5)/10);
	p = prs_CalcPressurePa();
	gZBuf[gnSmpCnt] = gnAltCm = prs_Pa2Cm(p);
	gnSmpCnt++;
	if (gnSmpCnt == SNS_NUM_Z_SAMPLES) 
	{
		gnSmpCnt = 0;
	}
	prs_AverageAltitude();
	prs_DeltaCmPerSec();
#if SENSOR_POST_LPF
	prs_LPFClimbRate();
#endif
#if UART_DBG
	sprintf(gszBuf,"Profile %dmS\r\n",tmr1_ElapsedTimeMs());
	uart_puts(gszBuf);
#endif
	*/
}   

void prs_AcquireAveragedSample(int nSamples)   
{
	s32 pa,tc,pAccum,tAccum,n;

	pAccum = 0;
	tAccum = 0;
	n = nSamples;

	while (n--)  
	{
		tc = bmp085_calc_temperature(bmp085_measure_temperature());
		pa = bmp085_calc_pressure(bmp085_measure_pressure());

		pAccum += pa;
		tAccum += tc;
	}

	tc = tAccum/nSamples;
	gnTempC = (tc >= 0 ?  (tc+5)/10 : (tc-5)/10);

	pa = pAccum/nSamples;
	p_lpf = pa << 5;

	gnAltCm = prs_Pa2Cm(pa);
	gnAltAvgM = (gnAltCm+50L)/100L;
	gnLaunchAltM = gnAltAvgM;

#if UART_DBG
	sprintf(gszBuf,"Averaged Temperature : %dC\r\n",gnTempC);
	uart_puts(gszBuf);
	sprintf(gszBuf,"Averaged Pressure : %ldpa\r\n",pa);
	uart_puts(gszBuf);
	sprintf(gszBuf,"Averaged Altitude : %ldm\r\n",gnAltAvgM);
	uart_puts(gszBuf);
#endif
}

#ifdef SENSOR_NOISE_EVAL

void prs_DumpAltitude(int nSamples, int pzInx)   
{
	/*
	s32 pa,tc,n;
	n = nSamples;
	while (n--)  
	{
		prs_TriggerTemperatureSample();
//		while(!gpioSnsEoc);
		while( ! (PINC & (1 << PC0)) );
		gnTraw = prs_ReadTemperatureSample();
		tc = prs_CalcTemperatureCx10();
		prs_TriggerPressureSample();
//		while(!gpioSnsEoc);
		while( ! (PINC & (1 << PC0)) );
		gnPraw = prs_ReadPressureSample();
		pa = prs_CalcPressurePa();
		gnTempC = (tc >= 0 ?  (tc+5)/10 : (tc-5)/10);
		gnAltCm = prs_Pa2Cm(pa);
#if UART_DBG
		//sprintf(gszBuf,"%ldpa\r\n",pa);
		//uart_puts(gszBuf);
		sprintf(gszBuf,"%ld\r\n",gnAltCm);
		uart_puts(gszBuf);
	}
#endif
	*/
}
#endif


void prs_AverageAltitude(void)   
{
	int cnt;
	s32 avg;
	cnt = SNS_NUM_Z_SAMPLES;
	avg = 0;
	while (cnt--)  
		avg += gZBuf[cnt];
	avg /= SNS_NUM_Z_SAMPLES;
	gnAltAvgCm = avg;
	gnAltAvgM = ((avg >= 0) ? (avg+50L)/100L : (avg-50L)/100L); 

	return; 
}


// m = (n*sum(xy) - sum(x)*sum(y)) / (n*sum(x*x) - sum(x)*sum(x))
//
void prs_DeltaCmPerSec(void)   
{
#if 1
	int n,tref,trel;
	s32 z;
	s64 num,sum_zt;

	tref = gnSmpCnt; // time origin is the current sample in window
	sum_zt = 0;
	n = SNS_NUM_Z_SAMPLES;

	while (n--)  
	{
		z = gZBuf[n] - gnAltAvgCm;
		trel = n-tref;

		if( n > tref ) 
			trel -= SNS_NUM_Z_SAMPLES;

		sum_zt += ((s64)trel*z);
	}

	num = ((s64)SNS_NUM_Z_SAMPLES) * sum_zt;
	gnCps = gnDen ? (s32)((num * SENSOR_SAMPLES_PER_SEC) / gnDen) : 0;
#else

	u8 n;
	s32 sum_zt,sum_z,sum_t,sum_t2,trel,num,den,tref,z;

	tref = gnSmpCnt; // time origin is the current sample in window
	sum_zt = sum_z = sum_t = sum_t2 = 0;
	n = SNS_NUM_Z_SAMPLES;

	while (n--)
	{
		z = gZBuf[n];

		trel = n-tref;

		if (n > tref) 
			trel -= SNS_NUM_Z_SAMPLES;

		sum_z += z;
		sum_t += trel;
		sum_t2 += (trel*trel);
		sum_zt += (trel*z);
	}

	num = (SNS_NUM_Z_SAMPLES * sum_zt) - (sum_z * sum_t);
	den = (SNS_NUM_Z_SAMPLES * sum_t2) - (sum_t * sum_t);

	gnCps = den ? (s16)((num * SENSOR_SAMPLES_PER_SEC) / den) : 0;

#endif

	return;
}


#if SENSOR_POST_LPF
void prs_LPFClimbRate(void)   
{
	s32 tmp;
	tmp = gnCpsx32*(32-gnKLPFC) + (gnCps<<5)*gnKLPFC;
	gnCpsx32 = (tmp >= 0 ? ((tmp + 16)>>5) : ((tmp - 16)>>5));
	gnCps = (gnCpsx32>>5);
}  
#endif // SENSOR_POST_LPF


s32 prs_Pa2Cm(s32 pa)  
{
	s32 inx,pa1,pa2,z1,z2,z;
	if (pa > PA_INIT) 
	{
		z = pgm_read_dword(&gPZTbl[0]);
	}
	else 
	{
		inx = (PA_INIT - pa)>>10;      
		if (inx >= PZLUT_ENTRIES) 
		{
			z = pgm_read_dword(&gPZTbl[PZLUT_ENTRIES-1]);
		}
		else 
		{
			pa2 = PA_INIT - (inx<<10);
			pa1 = (inx > 0 ? pa2+PA_DELTA : pa2);
			z1 = (inx > 0 ? pgm_read_dword(&gPZTbl[inx-1]) : pgm_read_dword(&gPZTbl[0]));
			z2 = pgm_read_dword(&gPZTbl[inx]);
			z = ((pa1-pa)*z2 + (pa-pa2)*z1)>>10;
		}
	}

	return z;
}

